#include <cmath>
#include "Eigen/Eigen"

/***
 * Calculate the distance between two 3d line segments and find out the nearest points. A line segment is defined by two end points.
 * Return false if the segment length is zero.
 * ***/
bool lineSegmentDistance(const Eigen::Vector3d& l1_p1, const Eigen::Vector3d& l1_p2, const Eigen::Vector3d& l2_p3, const Eigen::Vector3d& l2_p4,
                           Eigen::Vector3d &l1_nearest_p, Eigen::Vector3d &l2_nearest_p, double &distance)
{
    Eigen::Vector3d s1 = l1_p2 - l1_p1;
    Eigen::Vector3d s2 = l2_p4 - l2_p3;
    double s1_square_norm = s1.squaredNorm();
    double s2_square_norm = s2.squaredNorm();

    if(s1_square_norm == 0 || s2_square_norm == 0){
        return false;
    }

    double s1_s2_dot = s1.dot(s2);
    double denominator = s1_square_norm * s2_square_norm - s1_s2_dot * s1_s2_dot;
    double lambda_1 = (s1_s2_dot * (l1_p1 - l2_p3).dot(s2) - s2_square_norm * (l1_p1 - l2_p3).dot(s1)) / denominator;
    double lambda_2 = -(s1_s2_dot * (l1_p1 - l2_p3).dot(s1) - s1_square_norm * (l1_p1 - l2_p3).dot(s2)) / denominator;

    if(lambda_1 >= 0.0 && lambda_1 <= 1.0 && lambda_2 >= 0.0 && lambda_2 <= 1.0){
        l1_nearest_p = l1_p1 + lambda_1 * s1;
        l2_nearest_p = l2_p3 + lambda_2 * s2;
        distance = (l1_nearest_p - l2_nearest_p).norm();
    }else{
        // Find nearest distance by searching for endpoints
        double distance_square = (l1_p1 - l2_p3).squaredNorm();
        l1_nearest_p = l1_p1;
        l2_nearest_p = l2_p3;

        double distance_square_temp =  (l1_p1 - l2_p4).squaredNorm();
        if(distance_square_temp < distance_square){
            distance_square = distance_square_temp;
            l1_nearest_p = l1_p1;
            l2_nearest_p = l2_p4;
        }

        distance_square_temp =  (l1_p2 - l2_p3).squaredNorm();
        if(distance_square_temp < distance_square){
            distance_square = distance_square_temp;
            l1_nearest_p = l1_p2;
            l2_nearest_p = l2_p3;
        }

        distance_square_temp =  (l1_p2 - l2_p4).squaredNorm();
        if(distance_square_temp < distance_square){
            distance_square = distance_square_temp;
            l1_nearest_p = l1_p2;
            l2_nearest_p = l2_p4;
        }

        distance = sqrt(distance_square);
    }

    return true;

}

/***
 * Calculate the distance between a point and a 3d line segment and find out the nearest points. A line segment is defined by two end points.
 * Return false if the segment length is zero.
 * ***/
bool pointToLineSegmentDistance(const Eigen::Vector3d& l_p1, const Eigen::Vector3d& l_p2, const Eigen::Vector3d& p, Eigen::Vector3d& closest_point, double &distance){

    if((l_p1-l_p2).squaredNorm() < 0.0001){
        // Line segment is too short
        return false;
    }

    double param_t = ((l_p1(0)-p(0))*(l_p1(0)-l_p2(0)) + (l_p1(1)-p(1))*(l_p1(1)-l_p2(1)) +  (l_p1(2)-p(2))*(l_p1(2)-l_p2(2)))
            / ((l_p1(0) - l_p2(0))*(l_p1(0) - l_p2(0)) +  (l_p1(1) - l_p2(1))*(l_p1(1) - l_p2(1)) + (l_p1(2) - l_p2(2))*(l_p1(2) - l_p2(2)));

    if(param_t > 0.0 && param_t < 1.0){
        closest_point = l_p1 + param_t * (l_p2 - l_p1);
        distance = (closest_point - p).norm();
    }else{
        double distance1 = (l_p1 - p).norm();
        double distance2 = (l_p2 - p).norm();
        if(distance1 < distance2){
            closest_point = l_p1;
            distance = distance1;
        }else{
            closest_point = l_p2;
            distance = distance2;
        }
    }

    return true;
}



